#include <iostream>
#include <string>
#include <fstream>
#include <unistd.h>
#include <getopt.h>
#include <sys/stat.h>
#include <pangolin/pangolin.h>

#include <trajectory.h>

using namespace std;

void matlab_script_generate_all_traj(Gt_traj &gt_traj, vector<Es_traj> &es_trajs, Config &conf)
{
  string gt_O_name = gt_traj.output_speci_name_get();
  vector<string> es_O_names;
  string es_O_name;
  for (auto es_traj : es_trajs) {
    es_O_name = es_traj.output_speci_name_get();
    es_O_names.push_back(es_O_name);
  }

  if (access((conf.output_dir+"/matlab_scripts/").c_str(), 0)) {
    mkdir((conf.output_dir+"/matlab_scripts/").c_str(), 0775);
  }

  string matp_s = conf.output_dir+"/matlab_scripts/all_es2gt_matlab.m";
  ofstream matp;
  matp.open(matp_s, ios::out|ios::trunc);
  if (!matp.is_open()) {
    cout << "Failed open output file: " << matp_s << endl;
    cout << "Did you specify the output dir right?" << endl
         << "Use -h options get more information!" << endl;
    return;
  } else {
    cout << "Generate matlab scripts: " << matp_s << endl;
  }

  string gt_fname = "..\\data\\" + gt_O_name + "_trajectory.txt";

  matp << "coArray=['g','b','c','m','w','r','y'];" << endl
       << "figure(1);" << endl
       << "gt_file = importdata('" << gt_fname << "');" << endl
       << "gt = gt_file.data;" << endl
       << "plot3(gt(:,2), gt(:,3), gt(:,4),'k','linewidth',1);" << endl;

  for (int i = 0; i < es_O_names.size(); i++) {
    string es_O_name = es_O_names[i];
    string es_fname = "..\\data\\" + es_O_name + "_trajectories.txt";
    matp << "hold on;" << endl
         << es_O_name << "_file = importdata('" << es_fname << "');" << endl
         << es_O_name << " = " << es_O_name << "_file.data;" << endl
         << es_O_name << "_col = " << i + 1 << ";" << endl
         << "plot3(" << es_O_name << "(:,4), " << es_O_name << "(:,5), "
         << es_O_name << "(:,6), coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }
  matp << "axis equal;" << endl;

  if (conf.IS_USE_UMEYAMA) {
    matp << "grid on; hold off;" << endl
         << "legend('Groundtruth'";
    for (auto es_O_name : es_O_names) { matp << ",'" << es_O_name << "'"; }
    matp << ",'Interpreter','none');" << endl;
  } else {
    matp << "grid on; hold on;" << endl
         << "plot3(gt(1,2), gt(1,3), gt(1,4),'^k','MarkerFaceColor','k');" << endl
         << "hold off;" << endl
         << "legend('Groundtruth'";
    for (auto es_O_name : es_O_names) { matp << ",'" << es_O_name << "'"; }
    matp << ",'Starting point','Interpreter','none');" << endl;
  }

  matp << "title(\"Groundtruth of trajectory and estimated trajectories\");" << endl << endl
       << "figure(2);" << endl
       << "subplot(1,2,1);" << endl
       << "plot(gt(:,2), gt(:,4),'k','linewidth',1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "hold on;" << endl
         << "plot(" << es_O_name << "(:,4), " << es_O_name << "(:,6), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }
  matp << "title(\"Top view of trajectories\");" << endl
       << "axis equal;" << endl;

  if (conf.IS_USE_UMEYAMA) {
    matp << "grid on; hold off;" << endl;
  } else {
    matp << "grid on; hold on;" << endl
         << "plot(gt(1,2), gt(1,4),'^k','MarkerFaceColor','k');" << endl
         << "hold off;" << endl;
  }

  matp << "subplot(1,2,2);" << endl
       << "plot(gt(:,3), gt(:,4),'k','linewidth',1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "hold on;" << endl
         << "plot(" << es_O_name << "(:,5), " << es_O_name << "(:,6), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }

  matp << "title(\"Side view of trajectories\");" << endl
       << "axis equal;" << endl;

  if (conf.IS_USE_UMEYAMA) {
    matp << "grid on; hold off;" << endl
         << "legend('Groundtruth'";
    for (auto es_O_name : es_O_names) { matp << ",'" << es_O_name << "'"; }
      matp << ",'Orientation','horizontal','location',[0.13,0.01,0.78,0.05]"
          << ",'Interpreter','none');" << endl << endl;
  } else {
    matp << "grid on; hold on;" << endl
         << "plot(gt(1,3), gt(1,4),'^k','MarkerFaceColor','k');" << endl
         << "hold off;" << endl
         << "legend('Groundtruth'";
    for (auto es_O_name : es_O_names) { matp << ",'" << es_O_name << "'"; }
      matp << ",'Starting point','Orientation','horizontal','location',[0.13,0.01,0.78,0.05]"
          << ",'Interpreter','none');" << endl << endl;
  }

  matp << "figure(3);" << endl
       << "suptitle(\"Triaxial translocation\");" << endl
       << "subplot(3,1,1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "plot(" << es_O_name << "(:,8)-gt(1,1), " << es_O_name << "(:,4), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl
         << "hold on;" << endl;
  }
  matp << "plot(gt(:,1)-gt(1,1), gt(:,2),'k','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"X-axis\");" << endl

       << "subplot(3,1,2)" << endl
       << "plot(gt(:,1)-gt(1,1), gt(:,3),'k','linewidth',1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "hold on;" << endl
         << "plot(" << es_O_name << "(:,8)-gt(1,1), " << es_O_name << "(:,5), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }
  matp << "grid on; hold off;" << endl
       << "ylabel(\"Y-axis\");" << endl

       << "subplot(3,1,3)" << endl
       << "plot(gt(:,1)-gt(1,1), gt(:,4),'k','linewidth',1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "hold on;" << endl
         << "plot(" << es_O_name << "(:,8)-gt(1,1), " << es_O_name << "(:,6), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }

  matp << "grid on; hold off;" << endl
       << "ylabel(\"Z-axis\");" << endl
       << "xlabel(\"Running time, Unit(s)\"); " << endl
       << "legend('Groundtruth'";
  for (auto es_O_name : es_O_names) { matp << ",'" << es_O_name << "'"; }
  matp << ",'Orientation','horizontal','location',[0.13,0.01,0.78,0.05]"
       << ",'Interpreter','none');" << endl << endl

       << "figure(4);" << endl
       << "suptitle(\"Euler angles\");" << endl
       << "subplot(3,1,1);" << endl;
  for (auto es_O_name : es_O_names) {
    string ees_fname = "..\\data\\" + es_O_name + "_euler.txt";
    matp << es_O_name << "_file = importdata('" << ees_fname << "');" << endl
         << es_O_name << " = " << es_O_name << "_file.data;" << endl
         << "plot(" << es_O_name << "(:,13)-gt(1,1), " << es_O_name << "(:,4), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl
         << "hold on;" << endl;
  }
  matp << "plot(gt(:,1)-gt(1,1), gt(:,5),'k','linewidth',1);" << endl
  //matp << "plot(gt(:,1)-gt(1,1), gt(:,5)-gt(1,5),'k','linewidth',1);" << endl
       << "grid on; hold off;" << endl
       << "ylabel(\"Roll(\\circ)\");" << endl

       << "subplot(3,1,2)" << endl
       << "plot(gt(:,1)-gt(1,1), gt(:,6),'k','linewidth',1);" << endl;
       //<< "plot(gt(:,1)-gt(1,1), gt(:,6)-gt(1,6),'k','linewidth',1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "hold on;" << endl
         << "plot(" << es_O_name << "(:,13)-gt(1,1), " << es_O_name << "(:,5), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }
  matp << "grid on; hold off;" << endl
       << "ylabel(\"Pitch(\\circ)\");" << endl

       << "subplot(3,1,3)" << endl
       << "plot(gt(:,1)-gt(1,1), gt(:,7),'k','linewidth',1);" << endl;
       //<< "plot(gt(:,1)-gt(1,1), gt(:,7)-gt(1,7),'k','linewidth',1);" << endl;
  for (auto es_O_name : es_O_names) {
    matp << "hold on;" << endl
         << "plot(" << es_O_name << "(:,13)-gt(1,1), " << es_O_name << "(:,6), "
         << "coArray(" << es_O_name << "_col),'linewidth',1);" << endl;
  }

  matp << "grid on; hold off;" << endl
       << "ylabel(\"Yaw(\\circ)\");" << endl
       << "xlabel(\"Running time, Unit(s)\"); " << endl
       << "legend('Groundtruth'";
  for (auto es_O_name : es_O_names) { matp << ",'" << es_O_name << "'"; }
  matp << ",'Orientation','horizontal','location',[0.13,0.01,0.78,0.05]"
       << ",'Interpreter','none');" << endl << endl;

  matp.close();
}

int main(int argc, char *argv[])
{
  if (argc < 2) {
    cout << "\t Use 'align_trajectories -h/--help' to get more usage" << endl;
    return -1;
  }

  static struct option long_options[] = {
    {"align", 0, 0, 'a'},
    {"help", 0, 0, 'h'},
    {"scale-align", 0, 0, 's'},
    {"t-offset", 1, 0, 't'},
    {"euler-reverse", 0, 0, 'e'},
    {"rot-transpose", 0, 0, 'r'},
    {"display", 0, 0, 'p'},
    {"output", 1, 0, 'o'},
    {"consider-rotations", 2, 0, 'c'},
    {"ref", 1, 0, 0},
    {"use-umeyama", 0, 0, 1},
    {"use-salas", 0, 0, 2},
    {"use-frank", 0, 0, 3},
    {"plot-matchline", 0, 0, 5},
    {"generate-gnuplot-scripts", 0, 0, 6},
    {NULL, 0, NULL, 0}
  };

  Config conf;
  int option_index = 0;
  vector<string> es_files;
  string gt_file;
  int c, t = 1;
  while ((c = getopt_long(argc, argv, "ahst:erpo:c::",
             long_options, &option_index)) != -1) {
    switch (c) {
    case 0:
      gt_file = optarg;
      break;
    case 1:
      conf.IS_USE_UMEYAMA = true;
      break;
    case 2:
      conf.IS_USE_SALAS = true;
      break;
    case 3:
      conf.IS_USE_FRANK = true;
      break;
    case 'c':
      if (optarg) {
        conf.rot_weight = atof(optarg);
        cout << "Consider rotaion. Weight:" <<  conf.rot_weight << endl;
      }
      conf.IS_CONSIDER_ROT = true;

      break;
    case 5:
      conf.IS_PLOT_MATCHLINE = true;
      break;
    case 6:
      break;
    case 'a':
      conf.IS_ALIGN = true;
      break;
    case 's':
      conf.IS_ALIGN_SCALE = true;
      break;
    case 't':
      conf.t_offset = atof(optarg);
      break;
    case 'e':
      cout << "euler-reverse was set, Please input the sign vector for euler"
              "and press enter to end" << endl
           << "(Only support -1 or 1 (eg: 1 -1 1) , and the first will be used for groundtruth)"
           << endl << ":";
      while(cin >> t){
        conf.rs.push_back(t);
        if ('\n' == cin.get())
        {
            break;
        }
      }
      break;
    case 'r':
      conf.IS_ROT_TRANSPOSE = true;
      cout << "Rotation transpose in trajectory!" << endl;
      break;
    case 'p':
      conf.IS_DISPLAY = true;
      break;
    case 'o':
      conf.output_dir = optarg;
      conf.IS_OUTPUT = true;
      break;
    case 'h':
    default:
      cout << "Usage: " << endl;
      cout << "\t align_trajectories [opts]"\
        " --ref [groundtruth].txt [estimate].txt" << endl
        << "\t -a --align\t Align trajectories by calculating the transformation" << endl
        << "\t -h --help\t Print the instructions of this tool" << endl
        << "\t -s --scale-align\t Evaluate scale when aligning trajectories. This need -a." << endl
        << "\t -t --t-offset [timeoffset es-gt]\t Set the time offset. If the t-offset isn't set"
        << ", the t-offset will be calculated automatically" << endl
        << "\t -e --euler-reverse\t This option is deprecated !!! Use '-r' Instead!"
        << "Make Euler sign opposite, due to different pose definition."
        << "The sign vector for trajs U need to input, which default to be 1." << endl
        << "\t -r --rot-transpose\t The quaternion in trajectory will be transposed" << endl
        << "\t -p --display\t Use pangolin to display trajectories" << endl
        << "\t -o --output\t Output results of aligning" << endl
        << "\t -c --consider-rotations\t Add the rotations constraints in align algorithm, "
        << "\t only for the starting point alignment algorithm."
        << "\t You can specify the weight of rotaion by optiion '-c0.5'" << endl
        << endl
        << "\t --ref\t Indicates the file which saves groundtruth" << endl
        << "\t --use-umeyama\t Utilizes umeyama method for aligning trajectories" << endl
        << "\t --use-salas\t Utilizes salas method for aligning trajectories" << endl
        << "\t --use-frank\t Utilizes our method for aligning trajectories(RECOMMMED!)" << endl
        << "\t --plot-matchline\t Display matchline in pangolin" << endl;
        return 0;
    }
  }

  int txt_num = 0;
  if (optind < argc) {
    while (optind < argc) {
      if (!strcmp(argv[optind]+strlen(argv[optind])-4, ".txt")) {
        es_files.push_back(argv[optind]);
        txt_num++;
      }
      optind++;
    }
  }

  if (txt_num > 1 && gt_file.empty()) {
    cout << "--ref needed to specify the reference trajectory!" << endl
         << "\t Use 'align_trajectories -h/--help' to get more usage" << endl;
    return -1;
  }

  if (conf.rs.size() > 0 && txt_num != conf.rs.size()) {
    cout << "Euler reverse vector size not be right, some Euler of traj will not be reserse"
         << endl;
  }

  Gt_traj gt_traj;
  if (!gt_file.empty()) {
    cout << "processing groundtruth trajectory " << ": " << gt_file << endl;
    gt_traj = Gt_traj(gt_file, conf);
  }

  vector<Es_traj> es_trajs;
  for (int i = 0; i < es_files.size(); i++) {
    cout << "processing estimate trajectory " << i << ": " << es_files[i] << endl;
    Es_traj es_traj(es_files[i], &gt_traj, conf);
    es_trajs.push_back(es_traj);
  }

  if (conf.IS_OUTPUT && !gt_traj.stamp_poses.empty()) {
    matlab_script_generate_all_traj(gt_traj, es_trajs, conf);
  }

  if (!conf.IS_DISPLAY) {
    return 0;
  }

  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);


  auto ps = es_trajs[0].stamp_poses.begin()->second.second;
  auto pe = es_trajs[0].stamp_poses.rbegin()->second.second;
  if(gt_traj.stamp_poses.size() > 1) {
    ps = gt_traj.stamp_poses.begin()->second.second;
    pe = gt_traj.stamp_poses.rbegin()->second.second;
  }

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(ps.translation()[0], ps.translation()[1], ps.translation()[2],
       pe.translation()[0], pe.translation()[1], pe.translation()[2], 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
          .SetHandler(new pangolin::Handler3D(s_cam));


  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    if(gt_traj.stamp_poses.size() > 1) {
      gt_traj.display_locs();
    }

    for (int i = 0; i < es_trajs.size(); i++) {
      es_trajs[i].display_locs();
    }

#if 0
    for (int i = 0; i < es_trajs.size(); i++) {
      cout << es_trajs[i].file_name << endl;
      es_trajs[i].test();
    }
#endif

    pangolin::FinishFrame();
  }

  return 0;
}

